
/*
* core one, cooperator.
* proc message.
*/

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico.h"
#include "pico/multicore.h"

#include "umsg.h"

/*
* 0 = data[0],  init : [1] [2] [3] [4] = baud
* 1 = data[0],  read : [1] = addr, [2] = cnt
* 2 = data[0], write : [1] = addr, [...] = data
* 3 = data[0], general call : write..
*/
void p1_test_i2c( umsg_t * pmsg )
{
	i2c_inst_t * pi2c = i2c0;
	uint8_t * ptr = pmsg->data;
	uint32_t temp;
	uint8_t addr;
	int tlen;
	int iret;
	int i;
	uint8_t tary[32];
	uint8_t repy[4];


	switch ( ptr[0] ) {
	
	case 0:
		temp = ptr[4];
		temp = (temp << 8) | ptr[3];
		temp = (temp << 8) | ptr[2];
		temp = (temp << 8) | ptr[1];
		printf("i2c init, %u\n", temp );
		i2c_init( pi2c, temp );
		break;

	case 1:
		addr = ptr[1];
		tlen = ptr[2];
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );

		printf("read = %d\n", iret );
		
		if ( iret > 0 ) {
			int  i;
			for ( i=0; i<iret; i++ ) {
				printf( "%x : ", tary[i] );
			}
			printf( "\n\n");
		}

		break;

	case 2:
		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		iret = i2c_write_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		printf( "write, succ\n" );
		break;

	case 3:
		/* process call, write, restart, read */
		addr = ptr[1];
		tlen = pmsg->mlen - 3;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 3];
		}

		repy[0] = (uint8_t)tlen;
		iret = i2c_write_blocking( pi2c, addr, tary, tlen, true );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		tlen = ptr[2];
		repy[1] = (uint8_t)tlen;
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "read, ret = %d\n", iret );
			break;
		}

		repy[2] = 0x55;
		repy[3] = 0x66;
		break;
	
	case 4:
		/* StartByte, master send  */

		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		pi2c->hw->enable = 0;
		temp = I2C_IC_TAR_SPECIAL_VALUE_ENABLED << I2C_IC_TAR_SPECIAL_LSB;
		temp = temp | (I2C_IC_TAR_GC_OR_START_VALUE_START_BYTE << I2C_IC_TAR_GC_OR_START_LSB);
		temp = temp | addr;
		pi2c->hw->tar = temp;
		pi2c->hw->enable = 1;

		for ( i=0; i<tlen; i++ ) {

			temp = I2C_IC_DATA_CMD_CMD_VALUE_WRITE << I2C_IC_DATA_CMD_CMD_LSB;
			temp = temp | tary[i];

			if ( (i + 1) == tlen ) {
				temp = temp | I2C_IC_DATA_CMD_STOP_VALUE_ENABLE << I2C_IC_DATA_CMD_STOP_LSB;
			}
			
			pi2c->hw->data_cmd = temp;
		}

		/* check and wait */
		while ( true ) {

			temp = pi2c->hw->raw_intr_stat;

            if ( temp & I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS ) {
            	iret = 0x11;
                break;
            }

            if ( temp & I2C_IC_RAW_INTR_STAT_STOP_DET_BITS ) {
            	iret = 0;
                break;
            }

#if 0
            if ( (pi2c->hw->status & 0x1) == 0 ) {
            	iret = 0;
                break;
            }
#endif

		}

        temp = pi2c->hw->tx_abrt_source;
        if ( temp != 0 ) {
            // Note clearing the abort flag also clears the reason, and
            // this instance of flag is clear-on-read! Note also the
            // IC_CLR_TX_ABRT register always reads as 0.
            pi2c->hw->clr_tx_abrt;
            pi2c->hw->clr_intr;
		}
        
        repy[0] = temp & 0xFF;
        repy[1] = (temp >> 8) & 0xFF;
        repy[2] = (temp >> 16) & 0xFF;
        repy[3] = (temp >> 24) & 0xFF;

        /**/
        pi2c->hw->enable = 0;
        break;

	default:
		iret = -1;
		break;
	}

	pmsg->type = 0x10;
	pmsg->mlen = 5;
	pmsg->data[0] = (uint8_t)iret;
	pmsg->data[1] = repy[0];
	pmsg->data[2] = repy[1];
	pmsg->data[3] = repy[2];
	pmsg->data[4] = repy[3];
	multicore_fifo_push_blocking( (intptr_t)pmsg );
	return;
}


#include "hardware/resets.h"
#include "hardware/pio.h"
#include "hardware/pio_instructions.h"


/**/
static const uint16_t ws2812_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block
    0xe037, //  1: set    x, 24
    0xe301, //  2: set    pins, 1                [3]
    0x6401, //  3: out    pins, 1                [3]
    0xe200, //  4: set    pins, 0                [2]
    0x0042, //  5: jmp    x--, 2
    0x0000, //  6: jmp    0
            //     .wrap
};

static const struct pio_program ws2812_program = {
    .instructions = ws2812_program_instructions,
    .length = 7,
    .origin = -1,
};




int p1_test_pio( umsg_t * pmsg )
{
	PIO pio = pio0;
	uint sm = 0;
	uint opin = 16;
	pio_sm_config cfg;

	/* unreset */
	unreset_block_wait( RESETS_RESET_PIO0_BITS );

	/* gpio */
	pio_gpio_init( pio, opin );
	pio_sm_set_consecutive_pindirs(pio, sm, opin, 1, true );

	/* config */
	cfg = pio_get_default_sm_config();
	sm_config_set_out_pins( &cfg, opin, 1 );
	sm_config_set_set_pins( &cfg, opin, 1 );
	sm_config_set_in_pins( &cfg, opin );
	sm_config_set_sideset_pins( &cfg, opin );
	sm_config_set_sideset( &cfg, 0, 0, false );
	sm_config_set_clkdiv( &cfg, 12.5 );
	
	/* INST_MEM */
	pio_add_program_at_offset( pio, &ws2812_program, 0 );

	/**/
	pio_sm_init(pio, sm, 0, &cfg );
	pio_sm_set_enabled( pio, sm, true );

	/**/
	pmsg->type = 0x12;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );
	return 0;
}


int p1_test_pio_data( umsg_t * pmsg )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;

	/**/
	temp = *(uint32_t *)pmsg->data;
	temp = __RBIT(temp) >> 8;
	pio_sm_put_blocking( pio, sm, temp );

	/**/
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );	
	return 0;
}


static const uint16_t had2_program_instructions[] = {
            //     .wrap_target
    0x8020, //  0: push   block
    0x80a0, //  1: pull   block
    0x6025, //  2: out    x, 5
    0x7401, //  3: out    pins, 1         side 0 [4]
    0x1c43, //  4: jmp    x--, 3          side 1 [4]
    0x60a5, //  5: out    pc, 5
    
    0xf480, //  6: set    pindirs, 0      side 0 [4]
    0xbc42, //  7: nop                    side 1 [4]
    0x7425, //  8: out    x, 5            side 0 [4]
    0x5c01, //  9: in     pins, 1         side 1 [4]
    0xb442, // 10: nop                    side 0 [4]
    0x5b01, // 11: in     pins, 1         side 1 [3]
    0x004a, // 12: jmp    x--, 10
    0x8020, // 13: push   block
    0xb442, // 14: nop                    side 0 [4]
    0x5b01, // 15: in     pins, 1         side 1 [3]
    0xf081, // 16: set    pindirs, 1      side 0
    0xe401, // 17: set    pins, 1                [4]
    0x1c00, // 18: jmp    0               side 1 [4]
    
    0xf401, // 19: set    pins, 1         side 0 [4]
    0x7a25, // 20: out    x, 5            side 1 [2]
    0x6041, // 21: out    y, 1
    0x80a0, // 22: pull   block
    0x7401, // 23: out    pins, 1         side 0 [4]
    0x1c57, // 24: jmp    x--, 23         side 1 [4]
    0xa0e2, // 25: mov    osr, y
    0x7401, // 26: out    pins, 1         side 0 [4]
    0xbc42, // 27: nop                    side 1 [4]
    0xf401, // 28: set    pins, 1         side 0 [4]
    0x1c00, // 29: jmp    0               side 1 [4]
            //     .wrap
};


static const struct pio_program had2_program = {
    .instructions = had2_program_instructions,
    .length = 30,
    .origin = -1,
};


int had_init_pio( umsg_t * pmsg )
{
	PIO pio = pio0;
	uint sm = 0;
	uint clk_pin = 8;
	uint tms_pin = 9;
	pio_sm_config cfg;

	/* unreset */
	unreset_block_wait( RESETS_RESET_PIO0_BITS );

	/* gpio */
	gpio_pull_up( clk_pin );
	gpio_pull_up( tms_pin );
	gpio_pull_up( 10 );
	gpio_pull_up( 11 );
	pio_gpio_init( pio, clk_pin );
	pio_gpio_init( pio, tms_pin );
	pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true );
	pio_sm_set_consecutive_pindirs(pio, sm, tms_pin, 1, true );
	pio_sm_set_pins( pio, sm, (1 << clk_pin) | (1 <<tms_pin) );

	/* config */
	cfg = pio_get_default_sm_config();
	sm_config_set_out_pins( &cfg, tms_pin, 1 );
	sm_config_set_set_pins( &cfg, tms_pin, 1 );
	sm_config_set_in_pins( &cfg, tms_pin );
	sm_config_set_sideset_pins( &cfg, clk_pin );
	sm_config_set_sideset( &cfg, 2, true, false );
	sm_config_set_clkdiv( &cfg, 125 );

	/* INST_MEM */
	pio_add_program_at_offset( pio, &had2_program, 0 );

	/**/
	pio_sm_init( pio, sm, 1, &cfg );
	pio_sm_set_enabled( pio, sm, true );

	/**/
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );	
	return 0;
}


int had_null_clock( umsg_t * pmsg )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;

	/**/
	temp = 1;		/* next pc : 1 */
	temp = (temp << 22) | 0x2AAAAA;
	temp = (temp << 5) | 21;
	pio_sm_put_blocking( pio, sm, temp );
	pio_sm_put_blocking( pio, sm, temp );
	pio_sm_put_blocking( pio, sm, temp );

	/**/
	temp = 0;		/* next pc : 0 */
	temp = (temp << 22) | 0x2AAAAA;
	temp = (temp << 5) | 21;
	pio_sm_put_blocking( pio, sm, temp );

	/* wait finish */
	*(uint32_t *)pmsg->data = pio_sm_get_blocking( pio, sm );
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


/*
return : 0 校验正确, 其他表示校验错误.
*/
static int had_read_8b( uint8_t * pval )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;
	uint32_t pary;

	/**/
	temp = 7;					/* data bit cnt : 8 */
	temp = (temp << 5) | 6;		/* next pc : 6 (read_task) */
	temp = (temp << 4) | 0xA;	/* HACR rs=10, rw=1, start=0 */
	temp = (temp << 5) | 3;		/* reg select bits = 4 */
	pio_sm_put_blocking( pio, sm, temp );

	/**/
	temp = pio_sm_get_blocking( pio, sm );
	pary = pio_sm_get_blocking( pio, sm );
	*pval = (uint8_t)(temp >> 24);
	return (int)pary;
}


/*
return : 0 校验正确, 其他表示校验错误.
*/
static int had_read_32b( uint32_t * pval )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;
	uint32_t pary;

	/**/
	temp = 31;					/* data bit cnt : 8 */
	temp = (temp << 5) | 6;		/* next pc : 6 (read_task) */
	temp = (temp << 4) | 0xE;	/* rs=11, rw=1, start=0 */
	temp = (temp << 5) | 3;		/* reg select bits = 4 */
	pio_sm_put_blocking( pio, sm, temp );

	/**/
	temp = pio_sm_get_blocking( pio, sm );
	pary = pio_sm_get_blocking( pio, sm );
	*pval = temp;
	return (int)pary;
}



int had_msg_read_reg( umsg_t * pmsg )
{
	int iret;
	uint32_t rult;

	if ( pmsg->data[0] == 8 ) {

		uint8_t temp;

		iret = had_read_8b( &temp );
		rult = temp;

	} else if ( pmsg->data[0] == 32 ) {

		iret = had_read_32b( &rult );

	} else {
		iret = 0xEEeeEEee;
		rult = 0;
	}

	/**/
	*(uint32_t *)pmsg->data = iret;
	*(uint32_t *)(pmsg->data + 4)= rult;
	pmsg->mlen = 8;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}




static uint32_t had_write_8b( uint8_t data )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;

	/* odd parity : 1 bit */
	temp = __builtin_popcount( data ) + 1;
	temp = (temp << 5) | 7;		/* data bit cnt : 8 */
	temp = (temp << 5) | 19;	/* next pc : 19 (write_task) */
	temp = (temp << 4) | 0x8;	/* HACR rs=10, rw=0, start=0 */
	temp = (temp << 5) | 3;		/* reg select bits = 3 */
	pio_sm_put_blocking( pio, sm, temp );
	pio_sm_put_blocking( pio, sm, data );

	/**/
	return pio_sm_get_blocking( pio, sm );
}


static uint32_t had_write_32b( uint32_t data )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;

	/* odd parity : 1 bit */
	temp = __builtin_popcount( data ) + 1;
	temp = (temp << 5) | 31;	/* data bit cnt : 32 */
	temp = (temp << 5) | 19;	/* next pc : 19 (write_task) */
	temp = (temp << 4) | 0xC;	/* rs=11, rw=0, start=0 */
	temp = (temp << 5) | 3;		/* reg select bits : 4 */
	pio_sm_put_blocking( pio, sm, temp );
	pio_sm_put_blocking( pio, sm, data );

	/**/
	return pio_sm_get_blocking( pio, sm );
}


int had_msg_write_reg( umsg_t * pmsg )
{
	uint32_t rult;

	if ( pmsg->data[0] == 8 ) {

		uint8_t temp;

		temp = pmsg->data[1];
		rult = had_write_8b( temp );

	} else if ( pmsg->data[0] == 32 ) {

		uint32_t temp;

		temp = pmsg->data[4];
		temp = (temp << 8) | pmsg->data[3];
		temp = (temp << 8) | pmsg->data[2];
		temp = (temp << 8) | pmsg->data[1];

		rult = had_write_32b( temp);

	} else {
		rult = 0xEEeeEEee;
	}

	/**/
	*(uint32_t *)pmsg->data = rult;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_tttt_reg( umsg_t * pmsg )
{
	int iret;
	uint8_t temp;
	uint32_t rult;

	/**/
	temp = pmsg->data[0] & 0x1F;
	temp = 0x80 | temp;
	had_write_8b( temp );

	/**/
	iret = had_read_32b( &rult );
	*(uint32_t *)pmsg->data = iret;
	*(uint32_t *)(pmsg->data + 4)= rult;

	pmsg->mlen = 8;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;	
}


int had_msg_zzzz_reg( umsg_t * pmsg )
{
	int iret;
	uint8_t temp;
	uint32_t value;

	/**/
	temp = pmsg->data[0] & 0x1F;
	temp = 0x00 | temp;

	value = pmsg->data[4];
	value = (value << 8) | pmsg->data[3];
	value = (value << 8) | pmsg->data[2];
	value = (value << 8) | pmsg->data[1];

	had_write_8b( temp );
	had_write_32b( value );

	/**/
	*(uint32_t *)pmsg->data = 0;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}




void core1_main( void )
{
	umsg_t * pmsg;
	uint32_t cnt = 0;

	printf("hello, from, %d\n", sio_hw->cpuid );

	/* I2C */
	gpio_pull_up( 0 );
	gpio_pull_up( 1 );
	gpio_set_input_enabled( 0, true );
	gpio_set_input_enabled( 1, true );
	gpio_set_input_hysteresis_enabled( 0, true );
	gpio_set_input_hysteresis_enabled( 1, true );
	gpio_set_slew_rate( 0, GPIO_SLEW_RATE_SLOW );
	gpio_set_slew_rate( 1, GPIO_SLEW_RATE_SLOW );
	gpio_set_function( 0, GPIO_FUNC_I2C );
	gpio_set_function( 1, GPIO_FUNC_I2C );
	

	while ( 1 ) {

		/**/
		pmsg = (umsg_t *)(uintptr_t)multicore_fifo_pop_blocking();
		cnt += 1;

		// printf( "pmsg, %u: %p, %u, %u\n", cnt, pmsg, pmsg->type, pmsg->mlen );
		
		switch ( pmsg->type ) {

		case 0x10:
			p1_test_i2c( pmsg );
			break;

		case 0x11:
			/* echo */
			multicore_fifo_push_blocking( (intptr_t)pmsg );
			break;

		case 0x12:
			p1_test_pio( pmsg );
			break;
		
		case 0x13: 
			p1_test_pio_data( pmsg );
			break;

		case 0x16:
			had_init_pio( pmsg );
			break;

		case 0x17:
			had_null_clock( pmsg );
			break;

		case 0x18:
			had_msg_write_reg( pmsg );
			break;

		case 0x19:
			had_msg_read_reg( pmsg );
			break;

		case 0x20:
			had_msg_tttt_reg( pmsg );
			break;

		case 0x21:
			had_msg_zzzz_reg( pmsg );
			break;
			
		default:
			umsg_free( pmsg );
			break;
		}

		// test, echo..
		// multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
	}
}


